% Devin Koepl

clear('all');
close('all');
addpath('../animations', '../controllers', '../dynamics', '../environments', '../library');

phases = 10;
output = cell(phases, 2);

x0 = [ 0, 3, 1, 0, 0, 0, pi/2, 0, ActuatedModelClass.r0 * ActuatedModelClass.G / ActuatedModelClass.rl, 0];

% controller = ComControllerClass;
controller = SimpleControllerClass;

sim = ActuatedModelClass(x0, @flat_rigid_environment, controller);

for i = 1 : phases
   [output{i, 1}, output{i, 2}] = sim.run(); 
end

figure;
title('CoM Height');
xlabel('Time (s)');
ylabel('Height (m)');
hold on;
for i = 1 : phases
    plot(output{i, 1}, output{i, 2}(:, sim.yci), 'linewidth', 3);
end

figure;
title('CoM Velocity');
xlabel('Time (s)');
ylabel('Velocity (m/s)');
hold on;
for i = 1 : phases
    plot(output{i, 1}, output{i, 2}(:, sim.dxci), 'linewidth', 3);
end

% figure;
% title('Leg Motor Angle');
% xlabel('Time (s)');
% ylabel('Angle (rad)');
% hold on;
% for i = 1 : phases
%     plot(output{i, 1}, output{i, 2}(:, sim.oli), 'linewidth', 3);
% end

% Now animate the output.
anim = AnimateActuatedModelClass(sim.gnd, output);
anim.animate;
